#pragma once

#include "car.hpp"
#include "car_init.hpp"
#include "car_control.hpp"

#include "code_scan.hpp"

#include <iostream>
#include <thread>
#include <chrono>

#include "hwt101.hpp"
#include "control_node.hpp"
#include "pid.hpp"

#include <glog/logging.h>

#define LIFT_MAX_HEIGHT 0    // 最大高度  物理最大高度 回零处
#define LIFT_ROTATE_HEIGHT 5 // 旋转高度  可带着物料旋转
#define LIFT_TRAY_HEIGHT (65+20)  // 托盘高度  可抓起小车物料盘上的物料
#define LIFT_TABLE_HEIGHT (65+20) // 转盘高度  可抓起转盘上的物料
#define LIFT_OBJ_HEIGHT (78+20)   // 物体高度  可抓起物料上的物料
#define LIFT_GND_HEIGHT (150+20)  // 地面高度  可抓起地面上的物料
#define LIFT_MIN_HEIGHT 0    // 最小高度  物理最小高度

#define ROTATE_MAX_ANGLE 0          // 最大角度      物理最大角度 回零处
#define ROTATE_RIGHT_ANGLE -180     // 正右方角度    小车正右方角度 -148
#define ROTATE_TRAY0_ANGLE -35.5258 // 物料盘0角度   小车0号物料盘角度 -5.1
#define ROTATE_TRAY1_ANGLE -56.9969 // 物料盘1角度   小车1号物料盘角度 -25.1
#define ROTATE_TRAY2_ANGLE -98.7905 // 物料盘2角度   小车2号物料盘角度 -67.5
#define ROTATE_MIN_ANGLE 0          // 最小角度      物理最小角度

#define BRACKET_MAX_RANEG 0      // 最大距离  物理最大距离
#define BRACKET_FOLD_RANEG 20    // 折叠距离  此时机械臂折叠
#define BRACKET_TRAY0_RANEG (86.5+5) // 物料盘0   物料盘0距离78
#define BRACKET_TRAY1_RANEG (31.5+3) // 物料盘1   物料盘1距离23
#define BRACKET_TRAY2_RANEG (13+1)   // 物料盘2   物料盘2距离3
#define BRACKET_TABLE_RANEG 110  // 转盘距离  转盘上初次识别的距离
#define BRACKET_TARGET_RANEG 110 // 目标距离  距离色环靶标的距离
#define BRACKET_MIN_RANEG 0      // 最小距离  物理最小距离 回零处
#define BRACKET_AVOID_RANGE 90 //避免物理碰撞距离

#define waiting_dead_zone 20 // 等待死区

#define CLAW_MIN_ANGLE 42

#define CLAW_MAX_ANGLE 52

void stand_pick(int i, int j, int color,double &angle_t,double &claw_t);
bool put_obj(int obj, int height, double claw_y = BRACKET_TABLE_RANEG, double rotate_angle = -180);
void stand_put(int i, int j, int color, int h,double &angle_t,double &claw_t);
int step_get_color(int color_area_zone = 4000);
void get_ground_color(int get_ground_color[]);
void reset_mission();
bool pick_obj(int obj, int height);
double adjust_angle(double yaw,double angle);

